/*
 * Copyright (c) 2007, Nathan Sturtevant
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the University of Alberta nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY NATHAN STURTEVANT ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL <copyright holder> BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
package com.googlecode.pathmachine.mm;

import java.util.ArrayList;
import java.util.Iterator;
import java.util.List;
import java.util.logging.Logger;

/**
 * GenericAStar.java
 * 
 * This is a generic implementation of A* which will run on any problem which
 * has keys which fit into 32 bits. It makes as few assumptions as possible
 * about the underlying problem. It is used to search both the abstract and
 * low-level space for this demo code.
 * 
 * This implementation also has low-level access to the closed list and can be
 * run incrementally by initializing a search and then doing single steps of the
 * search one at a time. Combined, these make it easy to set up a visualization
 * of the search.
 * 
 * @author Nathan Sturtevant
 * @author <a href="mailto:eriq.adams@gmail.com">Eriq Muhammad Adams J.</a>
 */
public class GenericAStar {

	private static final Logger logger = Logger.getLogger(GenericAStar.class
			.getName());
	/**
	 * Number of nodes expanded
	 */
	private long nodesExpanded;
	/**
	 * number of nodes touched
	 */
	private long nodesTouched;
	/**
	 * Open Queue
	 */
	private GenericHeap openQueue = new GenericHeap();
	/**
	 * Open List
	 */
	private NodeLookupTable closedList = new NodeLookupTable();// openList
	/**
	 * Goal Node
	 */
	private int goal;
	/**
	 * Start Node
	 */
	private int start;
	/**
	 * Neighbors nodes
	 */
	private List<Integer> neighbors;
	/**
	 * SearcEnvironment
	 */
	private SearchEnvironment env;
	/**
	 * Print everything or not
	 */
	private boolean verbose = false;
	/**
	 * Maximum int value
	 */
	public static final int UINT32_MAX = 2147483647;

	/**
	 * getPath()
	 * 
	 * Find the optimal path between two states.
	 * 
	 * Will find an optimal path between two states, assuming that the heuristic
	 * function in the search environment is both admissible and consistent. (We
	 * do not re-open closed nodes.)
	 * 
	 * For efficiency purposes, we return the path in reverse order, since that
	 * is how it is extracted.
	 * 
	 * Since the search process can be run iteratively or in one fell swoop, we
	 * just use the iterative functions here as helper functions.
	 * 
	 * @param e
	 *            A search environment
	 * @param from
	 *            The start node
	 * @param to
	 *            The goal node
	 * @param thePath
	 *            A vector for the final path.
	 * @return none
	 */
	public void getPath(SearchEnvironment e, int from, int to,
			List<Integer> thePath) {
		if (verbose) {
			logger.info("--------------------------------");
		}
		if (!initializeSearch(e, from, to, thePath)) {
			return;
		}
		while (!doSingleSearchStep(thePath)) {
		}
	}

	/**
	 * initializeSearch()
	 * 
	 * Initialize an A* search.
	 * 
	 * @See getPath() for more details.
	 * 
	 * @param e
	 *            A search environment
	 * @param from
	 *            The start node
	 * @param to
	 *            The goal node
	 * @param thePath
	 *            A vector for the final path.
	 * @return true if the search initialized properly
	 */
	public boolean initializeSearch(SearchEnvironment e, int from, int to,
			List<Integer> thePath) {
		env = e;
		assert (openQueue.size() == 0);
		assert (closedList.size() == 0);
		nodesTouched = nodesExpanded = 0;
		start = from;
		goal = to;

		if ((from == UINT32_MAX) || (to == UINT32_MAX) || (from == to)) {
			thePath = new ArrayList<Integer>();
			return false;
		}
		SearchNode first = new SearchNode(env.HCost(goal, start), 0, start,
				start);
		openQueue.add(first);
		return true;
	}

	/**
	 * doSingleSearchStep()
	 * 
	 * Perform one node expansion in an A* search.
	 * 
	 * Initialize search must be called and return true before
	 * DoSingleSearchStep can be called. The function call will expand a single
	 * node in the A* search.
	 * 
	 * @See getPath() for more details.
	 * 
	 * @param thePath
	 *            The return path, returned only if the goal is found.
	 * @return true if the goal is found.
	 */
	public boolean doSingleSearchStep(List<Integer> thePath) {
		int currentOpenNode = UINT32_MAX;

		// if the open list is empty, no nodes are left to open
		if (openQueue.size() == 0) {
			closedList.clear();
			openQueue.reset();
			thePath = new ArrayList<Integer>(); // no path found!
			return true;
		}

		// get top of queue
		currentOpenNode = getNextNode();

		// if we have a goal, return the pasth
		if (env.goalTest(currentOpenNode, goal)) {
			extractPathToStart(currentOpenNode, thePath);
			closedList.clear();
			openQueue.reset();
			env = null;
			return true;
		}

		if (verbose) {
			// this print format won't work in general, but it works
			// for all the examples environments here
			logger.info("Opening " + (currentOpenNode >> 16) + ":"
					+ (currentOpenNode & 0xFFFF));
		}

		if (currentOpenNode == UINT32_MAX) {
			logger.info("Oh no! The current open node is NULL");
		}

		neighbors = env.getNeighbors(currentOpenNode);

		// iterate over all the children
		for (int x = 0; x < neighbors.size(); x++) {
			nodesTouched++;
			int neighbor = neighbors.get(x);
			assert (neighbor != UINT32_MAX);

			// it's already on the closed list
			if (closedList.containsKey(neighbor)) {
				if (verbose) {
					logger.info("skipping node" + (neighbor >> 16) + ":"
							+ (neighbor & 0xFFFF));
				}
				continue;
			} // it's on the open list
			else if (openQueue.isIn(new SearchNode(neighbor))) {
				if (verbose) {
					logger.info("updating node  " + (neighbor >> 16) + ":"
							+ (neighbor & 0xFFFF));
				}
				updateWeight(currentOpenNode, neighbor);
			} // it's newly opened
			else {
				if (verbose) {
					logger.info("adding node " + (neighbor >> 16) + ":"
							+ (neighbor & 0xFFFF));
				}
				addToOpenList(currentOpenNode, neighbor);
			}
		}
		return false;
	}

	/**
	 * checkNextNode()
	 * 
	 * Peek at the top of the open list.
	 * 
	 * @return The top node on the open list.
	 */
	public int checkNextNode() {
		return openQueue.top().currNode;
	}

	/**
	 * extractPathToStart()
	 * 
	 * Extract a path from the goal to the start. NOTE: The path is reversed
	 * (goal->start).
	 * 
	 * @param goalNode
	 *            The final node in the search
	 * @param thePath
	 *            The path between the goal and the start
	 * @return none
	 */
	public void extractPathToStart(int goalNode, List<Integer> thePath) {
		SearchNode n = null;
		if (closedList.containsKey(goalNode)) {
			n = closedList.get(goalNode);
		} else {
			n = openQueue.find(new SearchNode(goalNode));
		}

		do {
			thePath.add(n.currNode);
			n = closedList.get(n.prevNode);
		} while (n.currNode != n.prevNode);
		thePath.add(n.currNode);
	}

	/**
	 * getName()
	 * 
	 * Return the name of the algorithm
	 * 
	 */
	public String getName() {
		return "GenericAStar[]";
	}

	/**
	 * 
	 * @return number of expanded nodes
	 */
	public long getNodesExpanded() {
		return nodesExpanded;
	}

	/**
	 * 
	 * @return number of touched nodes
	 */
	public long getNodesTouched() {
		return nodesTouched;
	}

	/**
	 * 
	 * reset number of nodes
	 */
	public void resetNodeCount() {
		nodesExpanded = 0;
		nodesTouched = 0;
	}

	/**
	 * getMemoryUsage()
	 * 
	 * Get the combined size of the open & closed list.
	 * 
	 * @return The number of items in the open & closed list combined.
	 */
	public int getMemoryUsage() {
		return (int) closedList.size() + openQueue.size();
	}

	/**
	 * getNextNode()
	 * 
	 * Remove and return the top item from the open list.
	 * 
	 * The returned object is placed on the closed list after being removed from
	 * the open list.
	 * 
	 * @return node id of the top item on the open list
	 */
	private int getNextNode() {
		nodesExpanded++;
		int next;
		SearchNode it = openQueue.remove();
		next = it.currNode;
		closedList.put(next, it);
		return next;
	}

	/**
	 * updateWeight()
	 * 
	 * Check to see if we can update the weight of a node.
	 * 
	 * When searching the neighbors of the currOpenNode, if we come across a
	 * neighbor which is already opened, check to see if there is a shorter path
	 * to this node through the current open node. If so, update the parent of
	 * the neighbor and decrease its key in the open list.
	 * 
	 * @param currOpenNode
	 *            The possible new parent of the neighbor node
	 * @param neighbor
	 *            The node we might have a shorter path too
	 * @return none
	 */
	private void updateWeight(int currOpenNode, int neighbor) {
		SearchNode prev = openQueue.find(new SearchNode(neighbor));
		SearchNode alt = closedList.get(currOpenNode);
		double edgeWeight = env.GCost(currOpenNode, neighbor);
		double altCost = alt.gCost + edgeWeight + (prev.fCost - prev.gCost);
		if (FPUtil.fgreater(prev.fCost, altCost)) {
			if (verbose) {
				logger.info("Updating" + (neighbor >> 16) + ":"
						+ (neighbor & 0xFFFF) + " in openQueue, old f-cost is"
						+ prev.fCost + ", new f-cost is" + altCost);
			}
			prev.fCost = altCost;
			prev.gCost = alt.gCost + edgeWeight;
			prev.prevNode = currOpenNode;
			openQueue.decreaseKey(prev);
			// SearchNode test = openQueue.find(new SearchNode(neighbor));
		}
	}

	/**
	 * addToOpenList()
	 * 
	 * Add a new neighbor to the open list.
	 * 
	 * Add the neighbor node of the current open node to the open list.
	 * 
	 * @param currOpenNode
	 *            The parent of the new neighbor
	 * @param neighbor
	 *            The new node to add to the open list
	 * @return none
	 */
	private void addToOpenList(int currOpenNode, int neighbor) {
		double edgeWeight = env.GCost(currOpenNode, neighbor);
		SearchNode n = new SearchNode(closedList.get(currOpenNode).gCost
				+ edgeWeight + env.HCost(neighbor, goal), closedList
				.get(currOpenNode).gCost
				+ edgeWeight, neighbor, currOpenNode);
		if (verbose) {
			logger.info("Adding" + (neighbor >> 16) + ":" + (neighbor & 0xFFFF)
					+ " to openQueue, old size " + openQueue.size()
					+ ", f-cost is" + n.fCost);
		}
		openQueue.add(n);
	}

	/**
	 * getClosedListIter()
	 * 
	 * Get an iterator for the closed list.
	 * 
	 * @return the iterator
	 */
	public Iterator<Integer> getClosedListIter() {
		return closedList.keySet().iterator();
	}

	/**
	 * closedListIterNext()
	 * 
	 * Given the iterator, return the next node id
	 * 
	 * While the iterator could be used directly, it is better to use this
	 * function, as it hides the internal storage of the closed list.
	 * 
	 * @param it
	 *            An iterator returned from GenericAStar::GetClosedListIter()
	 * @return Next node id on the closed list.
	 */
	public int ClosedListIterNext(Iterator<Integer> it) {
		if (!it.hasNext()) {
			return UINT32_MAX;
		}
		int val = it.next();
		return val;
	}
}
